#include "Player.h"
#include <Windows.h>
#include "..\framework\InputManager.h"
#include "GameSettings.h"
#include <gl/glu.h>
#include "SubtitleManager.h"
#include "SoundManager.h"

// Player constants
const float_t RENDER_2D_FACTOR   = 1.0f;
const float_t INIT_SPEED         = 15.0f;
const float_t MOVEMENT_BUFFER    = 10.0f;
const float_t CAMERA_HEIGHT      = 5.0f;
const float_t CAMERA_PITCH       = 30.0f;
const float_t PLAYER_JUMP_FORCE  = 4000;
const float_t DEATH_TRIGGER_Y    = -100.0f;
const float_t MAX_DASH_TIME      = 0.2f;
const float_t DASH_SPEED         = 100.0f;
const float_t TORQUE_FACTOR      = 50.0f;
const float_t ILLEGAL_HEIGHT     = -100000;
const float_t ROTATION_FACTOR_2D = 4.0f;
const float_t JUMP_REQUEST_MAX   = 0.25f;

float_t WINDOW_X      = -32;
float_t WINDOW_Y      = -24;
float_t WINDOW_WIDTH  = 64;
float_t WINDOW_HEIGHT = 48;

// Ctor
Player::Player()
	:
	 m_fTotalRotation(0.0f),
	 m_bDoubleJump(false),
	 m_bApplyTorque(false),
	 m_bDashEnabled(false),
	 m_fDashTime(0.0f),
	 m_bStartPressed(false),
	 m_b2dEnabled(false),
	 m_bJumpAnimation(false),
	 m_bClearForceFlag(false),
	 m_bWinState(false),
	 m_fCacheX(0.0f),
	 m_fDeltaX(0.0f),
	 m_fCacheY(0.0f),
	 m_fDeltaY(0.0f),
	 m_fDimensionOffset(0.0f)
{
	// Init physics object
	m_physicsObject = new ImpossiblePhysics::PhysicsObj();
	m_physicsObject->idString = ImpossiblePhysics::PLAYER_OBJECT;
	m_fSpeed = INIT_SPEED;
	m_fDeltaX = 1.5f;
}


Player::~Player()
{
	delete m_physicsObject->body->getMotionState();
	delete m_physicsObject->body->getCollisionShape();

	delete m_physicsObject->body;
	delete m_physicsObject;
}

void Player::SetLookAt()
{
	// Reset model-view matrix
	glLoadIdentity();

	btTransform t;
	m_physicsObject->body->getMotionState()->getWorldTransform(t);
	btVector3 o = t.getOrigin();

	M3DMatrix44f m;
	m3dTranslationMatrix44(m, o.x(), o.y(), o.z());
	m3dInvertMatrix44(m, m);

	// Position the camera behind and above the player, then tilt it down
	glTranslatef(0.0f, -2, -10);
	glRotatef(CAMERA_PITCH, 1.0f, 0, 0);

	glMultMatrixf(m);
}

ImpossiblePhysics::PhysicsObj*& Player::GetPhysicsEntity()
{
	return m_physicsObject;
}

void Player::MoveForward(float_t argDeltaTime)
{
	if(!m_b2dEnabled)
	{
		// Move Player forward
		m_physicsObject->body->translate(btVector3(0, 0, -m_fSpeed*argDeltaTime));
	}
}

void Player::MoveBackward(float_t argDeltaTime)
{
	if(!m_b2dEnabled)
	{
		// Move Player back
		m_physicsObject->body->translate(btVector3(0, 0, m_fSpeed*argDeltaTime));
	}
}

void Player::SetWinState()
{
	m_bWinState = true;
}

float_t Player::GetCacheX() const
{
	//return -m_fCacheX;

	btTransform _transform;
	btVector3 _origin;
	m_physicsObject->body->getMotionState()->getWorldTransform(_transform);
	_origin = _transform.getOrigin();
	btScalar _z = _origin.z();

	return _z;
}

void Player::MoveRight(float_t argDeltaTime)
{
	if(!m_b2dEnabled)
	{
		m_physicsObject->body->translate(btVector3(m_fSpeed*argDeltaTime, 0, 0));
	}
	else
	{
		m_physicsObject->body->translate(btVector3(0, 0, -m_fSpeed*argDeltaTime));
	}
}

void Player::MoveLeft(float_t argDeltaTime)
{
	if(!m_b2dEnabled)
	{
		m_physicsObject->body->translate(btVector3(-m_fSpeed*argDeltaTime, 0, 0));
	}
	else
	{
		m_physicsObject->body->translate(btVector3(0, 0, m_fSpeed*argDeltaTime));
	}
}

void Player::ProcessInput(float_t argDeltaTime)
{
	if(InputManagerC::GetInstance()->IsMovingForward())
	{
		MoveForward(argDeltaTime);
	}

	if(InputManagerC::GetInstance()->IsMovingBackward())
	{
		MoveBackward(argDeltaTime);
	}

	if(InputManagerC::GetInstance()->IsMovingRight())
	{
		MoveRight(argDeltaTime);
	}

	if(InputManagerC::GetInstance()->IsMovingLeft())
	{
		MoveLeft(argDeltaTime);
	}

	if(InputManagerC::GetInstance()->IsJumping())
	{
		if(!m_bStartPressed)
		{
			m_physicsObject->body->setActivationState(DISABLE_DEACTIVATION);
			m_bStartPressed = true;
			m_bJumpRequested = false;
		}
		else
		{
			m_bJumpRequested = true; // Produce jump request
			m_fJumpTimer = 0.0f;
		}
	}

	if(InputManagerC::GetInstance()->IsDashing())
	{
		Dash(argDeltaTime);
	}
}

void Player::Update(float_t argDeltaTime)
{
	// Save player's origin this frame
	CacheOrigin();
	ProcessInput(argDeltaTime);
	UpdateDash(argDeltaTime);

	// Enable double jump if the player has landed
	if(m_physicsObject->hit)
	{
		m_bDoubleJump = true;
		ResetTransform();
		m_physicsObject->body->setAngularFactor(0.0f);
		m_physicsObject->body->setAngularVelocity(btVector3(0, 0, 0));

		if(m_bJumpRequested)
		{
			Jump(argDeltaTime);
		}
	}
	else if(m_bDoubleJump)
	{
		if(m_bJumpRequested)
		{
			Jump(argDeltaTime);
		}
	}

	// Check for death conditions
	if(m_physicsObject->bottom < DEATH_TRIGGER_Y)
	{
		ResetPlayer();

		if(!m_bWinState)
		{
			SubtitleManager::GetInstance()->IncreaseAttemptTimes(1);
		}
		// If player has won and kills himself, reset attempt count
		else
		{
			SubtitleManager::GetInstance()->SetAttemptTimes(0);
			m_bWinState = false;
		}

	}
	// Player collided with kill object
	if(m_physicsObject->dead)
	{
		ResetPlayer();

		if(!m_bWinState)
		{
			SubtitleManager::GetInstance()->IncreaseAttemptTimes(1);
		}
		else
		{
			// If player has won and kills himself, reset attempt count
			SubtitleManager::GetInstance()->SetAttemptTimes(0);
			m_bWinState = false;
		}
	}
	
	if(m_b2dEnabled)
	{
		//Move the camera if we're in 2D

		glMatrixMode(GL_PROJECTION);
		glLoadIdentity();
		WINDOW_X += m_fDeltaX;
		WINDOW_Y += m_fDeltaY;
		glOrtho(WINDOW_X, WINDOW_X + WINDOW_WIDTH, WINDOW_Y+ WINDOW_HEIGHT, WINDOW_Y, 0, 1);
		glMatrixMode(GL_MODELVIEW);

		// Lock the player's movement on the (3d) x axis

		btTransform t;
		m_physicsObject->body->getMotionState()->getWorldTransform(t);
		btVector3 o = t.getOrigin();

		btScalar _xOffset = o.x();

		m_physicsObject->body->translate(btVector3(-_xOffset, 0, 0));
	}

	// If there was is a jump request that has not been consumed, update the timer for it
	if(m_bJumpRequested)
	{
		m_fJumpTimer += argDeltaTime;
	}
	// Sorry, jump request has timed out...
	if(m_fJumpTimer > JUMP_REQUEST_MAX)
	{
		m_bJumpRequested = false;
	}

	PlayJumpAnimation(argDeltaTime);
}
void Player::ResetPlayer()
{
	 //m_fTotalRotation = 0.0f;
	 //m_bDoubleJump    = false;
	 //m_bStartPressed  = false;
	 //m_fCacheX = 0.0f;
	 //m_fDeltaX = 0.0f;
	 //m_fCacheY = 0.0f;
	 //m_fDeltaY = 0.0f;

	btTransform _transform;
	_transform.setOrigin(btVector3(0.0f, 5.0f, -2.0f));
	//_transform.setOrigin(btVector3(0.0f, 20.0f, -1900.0f));

	m_physicsObject->body->setCenterOfMassTransform(_transform);

	m_physicsObject->dead = false;
	ResetTransform();
	m_physicsObject->body->setLinearVelocity(btVector3(0, 0, 0));

	// Reset camera with player if in 2D
	if(m_b2dEnabled)
	{
		btTransform t;
		m_physicsObject->body->getMotionState()->getWorldTransform(t);
		btVector3 o = t.getOrigin();

		btVector3 _halfWidths = ((btBoxShape*)m_physicsObject->body->getCollisionShape())->getHalfExtentsWithMargin();

		float_t halfWidth = _halfWidths.x() * RENDER_2D_FACTOR;
		float_t x = (o.z() * -RENDER_2D_FACTOR) - halfWidth;
		float_t y = (o.y() * -RENDER_2D_FACTOR) - halfWidth;

		WINDOW_X = -32 + (x - 1.5f);
		m_fCacheX = x;

		WINDOW_Y = -24 + y;
		m_fCacheY = y;

		glMatrixMode(GL_PROJECTION);
		glLoadIdentity();
		glOrtho(WINDOW_X, WINDOW_X + WINDOW_WIDTH, WINDOW_Y+ WINDOW_HEIGHT, WINDOW_Y, 0, 1);
		glMatrixMode(GL_MODELVIEW);  
	}
}

void Player::CacheOrigin()
{
	btTransform t;
	m_physicsObject->body->getMotionState()->getWorldTransform(t);
	btVector3 o = t.getOrigin();

	// Calculate player's bottom border
	btVector3 _halfWidths = ((btBoxShape*)m_physicsObject->body->getCollisionShape())->getHalfExtentsWithMargin();
	m_physicsObject->bottom = o.y() - _halfWidths.y();

	// Save translated x,y position if we're in 2d so we can scroll camera
	if(m_b2dEnabled)
	{
		float_t _halfWidthX = _halfWidths.x() * RENDER_2D_FACTOR;
		float_t _halfWidthY = _halfWidths.y() * RENDER_2D_FACTOR;
		float_t _currentX = (o.z() * -RENDER_2D_FACTOR) - _halfWidthX;
		float_t _currentY = (o.y() * -RENDER_2D_FACTOR) - _halfWidthY;


		m_fDeltaX = _currentX - m_fCacheX;
		m_fCacheX = _currentX;

		m_fDeltaY = _currentY - m_fCacheY;
		m_fCacheY = _currentY;
	}
}


void Player::Jump(float_t argDelta)
{
	//m_bDoubleJump = false;
	if(m_physicsObject->hit)
	{
		m_physicsObject->body->applyCentralForce(btVector3(0.0f, PLAYER_JUMP_FORCE, 0));
		m_bJumpRequested = false; // Consume jump request
		m_bApplyTorque = true;
		m_bDoubleJump = true;

		// Manual rotation for 2D
		if(m_b2dEnabled)
		{
			m_bJumpAnimation = true;
			m_fTotalRotation = 0.0f;
		}
		SoundManager::GetInstance()->PlaySFXSound(SN_SFX_JUMP);
	}
	else if(m_bDoubleJump)
	{
		m_physicsObject->body->setLinearVelocity(btVector3(0, 0, 0));

		m_physicsObject->body->applyCentralForce(btVector3(0.0f, PLAYER_JUMP_FORCE, 0));
		m_bJumpRequested = false; // Consume jump request
		m_bApplyTorque = true;
		m_bDoubleJump = false;

		OutputDebugStringA("Jump requested\n");

		// Manual rotation for 2d
		if(m_b2dEnabled)
		{
			m_bJumpAnimation = true;
			m_fTotalRotation = 0.0f;
		}
		SoundManager::GetInstance()->PlaySFXSound(SN_SFX_JUMP);
	}
}

void Player::Dash(float_t argDelta)
{
	if(!m_bDashEnabled)
	{
		m_bDashEnabled = true;
		SoundManager::GetInstance()->PlaySFXSound(SN_SFX_DASH);
	}
}

void Player::UpdateDash(float_t argDelta)
{
	if(m_bDashEnabled)
	{
		if(m_fDashTime < MAX_DASH_TIME)
		{
			m_physicsObject->body->translate(btVector3(0, 0, -DASH_SPEED * argDelta));
			m_fDashTime += argDelta;
		}
		else
		{
			m_bDashEnabled = false;
			m_fDashTime = 0.0f;
		}
	}
}

void Player::SetMode3D(bool8_t arg3D)
{
	m_b2dEnabled = !arg3D;

	// Disable angular rotations in 2D
	if(m_b2dEnabled)
	{
		ResetTransform();
		m_physicsObject->body->setLinearVelocity(btVector3(0, 0, 0));

		// Set the player's transform to 0 depth
		btTransform t;
		m_physicsObject->body->getMotionState()->getWorldTransform(t);
		btVector3 o = t.getOrigin();
		t.setOrigin(btVector3(0, o.y(), o.z()));
		m_physicsObject->body->setCenterOfMassTransform(t);
	}
	else
	{
		// Reset the projection
		glMatrixMode (GL_PROJECTION);										// Select The Projection Matrix
		glLoadIdentity();													// Reset The Projection Matrix
		gluPerspective(45.0f,(GLfloat)Settings::WINDOW_WIDTH_3D/(GLfloat)Settings::WINDOW_HEIGHT_3D,0.1f,100.0f);

		glMatrixMode (GL_MODELVIEW);
		
		// Set the player's transform to platform's 3D depth
		btTransform t;
		m_physicsObject->body->getMotionState()->getWorldTransform(t);
		btVector3 o = t.getOrigin();
		t.setOrigin(btVector3(m_physicsObject->depth, o.y(), o.z()));
		m_physicsObject->body->setCenterOfMassTransform(t);
	}
}

void Player::PlayJumpAnimation(float_t argDelta)
{
	// For 2D, apply manual rotation
	if(m_b2dEnabled)
	{
		if(!m_bJumpAnimation) return;

		// Check for full rotation
		if(m_fTotalRotation >= M3D_PI) 
		{	
			// Calculate 
			m_fTotalRotation = 0.0f;
			m_bJumpAnimation = false;
			return;
		}

		// Update rotation value
		m_fTotalRotation += ((float_t)argDelta) * ROTATION_FACTOR_2D;

		// Clamp rotation value
		if(m_fTotalRotation >= M3D_PI)
		{
			m_fTotalRotation = M3D_PI;
		}

		btTransform _transform;
		_transform = m_physicsObject->body->getWorldTransform();

		btQuaternion _quaternion;
		_quaternion.setEuler(0.0f, m_fTotalRotation, 0.0f);
		_transform.setRotation(_quaternion);

		m_physicsObject->body->setCenterOfMassTransform(_transform);
	}
	// For 3D, apply torque
	else
	{
		if(!m_physicsObject->hit)
		{
			if(m_bApplyTorque)
			{
				m_physicsObject->body->setAngularFactor(Settings::PLAYER_ANGULAR_FACTOR);
				m_physicsObject->body->applyTorque(btVector3(-TORQUE_FACTOR, 0.0f, 0.0f));

				m_bApplyTorque = false;
			}
		}
	}
}

void Player::ResetTransform()
{
 	btTransform _transform;
	_transform = m_physicsObject->body->getWorldTransform();

	btQuaternion _quaternion;
	_quaternion.setEuler(0.0f, 0.0f, 0.0f);
	_transform.setRotation(_quaternion);

	m_physicsObject->body->setCenterOfMassTransform(_transform);
}

void Player::Draw() const
{
	glPushMatrix();
	// Transform modelview matrix by the rigid body's position
	btTransform _transform;
	float_t _mat[16];

	m_physicsObject->body->getMotionState()->getWorldTransform(_transform);
	_transform.getOpenGLMatrix(_mat);
	glMultMatrixf(_mat);

	// Set the drawing mode to wireframe
	glPolygonMode(GL_FRONT_AND_BACK, GL_LINE);

	// Draw cube
	glBegin(GL_QUADS);    

	// Grab dimensions
	btVector3 _halfWidths = ((btBoxShape*)m_physicsObject->body->getCollisionShape())->getHalfExtentsWithMargin();

	// Top face
	glColor3f(0.3f,0.3f,0.3f);
	glNormal3f(0.0f, 1.0f, 0.0f);
	glVertex3f( _halfWidths.x(), _halfWidths.y(),-_halfWidths.z());      
	glVertex3f(-_halfWidths.x(), _halfWidths.y(),-_halfWidths.z());      
	glVertex3f(-_halfWidths.x(), _halfWidths.y(), _halfWidths.z());      
	glVertex3f( _halfWidths.x(), _halfWidths.y(), _halfWidths.z());      
											 				  
	// Bottom face							 				  
	glColor3f(1.0f,0.5f,0.0f);
	glNormal3f(0.0f, -1.0f, 0.0f);
	glVertex3f( _halfWidths.x(),-_halfWidths.y(), _halfWidths.z());      
	glVertex3f(-_halfWidths.x(),-_halfWidths.y(), _halfWidths.z());      
	glVertex3f(-_halfWidths.x(),-_halfWidths.y(),-_halfWidths.z());      
	glVertex3f( _halfWidths.x(),-_halfWidths.y(),-_halfWidths.z());      
											 				  
	// Front face							 				  
	glColor3f(_halfWidths.x(),0.0f,0.0f);        			  
	glNormal3f(0.0f, 0.0f, 1.0f);
	glVertex3f( _halfWidths.x(), _halfWidths.y(), _halfWidths.z());      
	glVertex3f(-_halfWidths.x(), _halfWidths.y(), _halfWidths.z());      
	glVertex3f(-_halfWidths.x(),-_halfWidths.y(), _halfWidths.z());      
	glVertex3f( _halfWidths.x(),-_halfWidths.y(), _halfWidths.z());      
											 				  
	// Back face							 				  
	glColor3f(1.0f,0.0f,0.0f);         
	glNormal3f(0.0f, 0.0f, -1.0f);
	glVertex3f( _halfWidths.x(),-_halfWidths.y(),-_halfWidths.z());      
	glVertex3f(-_halfWidths.x(),-_halfWidths.y(),-_halfWidths.z());      
	glVertex3f(-_halfWidths.x(), _halfWidths.y(),-_halfWidths.z());      
	glVertex3f( _halfWidths.x(), _halfWidths.y(),-_halfWidths.z());      
											 				  
	// Left face							 				  
	glColor3f(0.0f,0.0f,1.0f);        			  
	glNormal3f(-1.0f, 0.0f, 0.0f);
	glVertex3f(-_halfWidths.x(), _halfWidths.y(), _halfWidths.z());      
	glVertex3f(-_halfWidths.x(), _halfWidths.y(),-_halfWidths.z());      
	glVertex3f(-_halfWidths.x(),-_halfWidths.y(),-_halfWidths.z());      
	glVertex3f(-_halfWidths.x(),-_halfWidths.y(), _halfWidths.z());      
											 				  
	// Right face							 				  
	glColor3f(1.0f,0.0f,1.0f);          
	glNormal3f(1.0f, 0.0f, 0.0f);
	glVertex3f( _halfWidths.x(), _halfWidths.y(),-_halfWidths.z());      
	glVertex3f( _halfWidths.x(), _halfWidths.y(), _halfWidths.z());      
	glVertex3f( _halfWidths.x(),-_halfWidths.y(), _halfWidths.z());      
	glVertex3f( _halfWidths.x(),-_halfWidths.y(),-_halfWidths.z());

	glEnd();

	glPolygonMode( GL_FRONT_AND_BACK, GL_FILL );

	// Draw cube
	glBegin(GL_QUADS);    

	// Grab dimensions
	_halfWidths = ((btBoxShape*)m_physicsObject->body->getCollisionShape())->getHalfExtentsWithoutMargin();

	// Color
	//glColor4ub(0x00, 0x00, 0xFF, 0xFF);

	// Top face
	//glColor3f(0.3f,0.3f,0.3f);
	glNormal3f(0.0f, 1.0f, 0.0f);
	glVertex3f( _halfWidths.x(), _halfWidths.y(),-_halfWidths.z());      
	glVertex3f(-_halfWidths.x(), _halfWidths.y(),-_halfWidths.z());      
	glVertex3f(-_halfWidths.x(), _halfWidths.y(), _halfWidths.z());      
	glVertex3f( _halfWidths.x(), _halfWidths.y(), _halfWidths.z());      
											 				  
	// Bottom face							 				  
	//glColor3f(1.0f,0.5f,0.0f);
	glNormal3f(0.0f, -1.0f, 0.0f);
	glVertex3f( _halfWidths.x(),-_halfWidths.y(), _halfWidths.z());      
	glVertex3f(-_halfWidths.x(),-_halfWidths.y(), _halfWidths.z());      
	glVertex3f(-_halfWidths.x(),-_halfWidths.y(),-_halfWidths.z());      
	glVertex3f( _halfWidths.x(),-_halfWidths.y(),-_halfWidths.z());      
											 				  
	// Front face							 				  
	//glColor3f(_halfWidths.x(),0.0f,0.0f);        			  
	glNormal3f(0.0f, 0.0f, 1.0f);
	glVertex3f( _halfWidths.x(), _halfWidths.y(), _halfWidths.z());      
	glVertex3f(-_halfWidths.x(), _halfWidths.y(), _halfWidths.z());      
	glVertex3f(-_halfWidths.x(),-_halfWidths.y(), _halfWidths.z());      
	glVertex3f( _halfWidths.x(),-_halfWidths.y(), _halfWidths.z());      
											 				  
	// Back face							 				  
	//glColor3f(1.0f,0.0f,0.0f);         
	glNormal3f(0.0f, 0.0f, -1.0f);
	glVertex3f( _halfWidths.x(),-_halfWidths.y(),-_halfWidths.z());      
	glVertex3f(-_halfWidths.x(),-_halfWidths.y(),-_halfWidths.z());      
	glVertex3f(-_halfWidths.x(), _halfWidths.y(),-_halfWidths.z());      
	glVertex3f( _halfWidths.x(), _halfWidths.y(),-_halfWidths.z());      
											 				  
	// Left face							 				  
	//glColor3f(0.0f,0.0f,1.0f);        			  
	glNormal3f(-1.0f, 0.0f, 0.0f);
	glVertex3f(-_halfWidths.x(), _halfWidths.y(), _halfWidths.z());      
	glVertex3f(-_halfWidths.x(), _halfWidths.y(),-_halfWidths.z());      
	glVertex3f(-_halfWidths.x(),-_halfWidths.y(),-_halfWidths.z());      
	glVertex3f(-_halfWidths.x(),-_halfWidths.y(), _halfWidths.z());      
											 				  
	// Right face							 				  
	//glColor3f(1.0f,0.0f,1.0f);          
	glNormal3f(1.0f, 0.0f, 0.0f);
	glVertex3f( _halfWidths.x(), _halfWidths.y(),-_halfWidths.z());      
	glVertex3f( _halfWidths.x(), _halfWidths.y(), _halfWidths.z());      
	glVertex3f( _halfWidths.x(),-_halfWidths.y(), _halfWidths.z());      
	glVertex3f( _halfWidths.x(),-_halfWidths.y(),-_halfWidths.z());

	glEnd();

	glPopMatrix();
}

void Player::Draw2D()
{
	glLoadIdentity();

	btTransform t;
	m_physicsObject->body->getMotionState()->getWorldTransform(t);


	btVector3 o = t.getOrigin();

	// Render wireframe

	btVector3 _halfWidths = ((btBoxShape*)m_physicsObject->body->getCollisionShape())->getHalfExtentsWithMargin();

	_halfWidths.setX(_halfWidths.x() * 1.2f);
	_halfWidths.setY(_halfWidths.y() * 1.2f);
	_halfWidths.setZ(_halfWidths.z() * 1.2f);

	// Find box "radius"
	float_t _radius = sqrtf(powf(_halfWidths.z(), 2) + powf(_halfWidths.y(), 2));

	// Calculate rotated points
	float_t x1 = o.z() - _radius * cosf(m_fTotalRotation + (M3D_PI / 4.0f)); // subtraction here because positive z is negative
	float_t y1 = o.y() - _radius * sinf(m_fTotalRotation + (M3D_PI / 4.0f));

	float_t x2 = o.z() - _radius * cosf(m_fTotalRotation + ((3.0f * M3D_PI) / 4.0f)); // subtraction here because positive z is negative
	float_t y2 = o.y() - _radius * sinf(m_fTotalRotation + ((3.0f * M3D_PI) / 4.0f));

	float_t x3 = o.z() - _radius * cosf(m_fTotalRotation + ((5.0f * M3D_PI) / 4.0f)); // subtraction here because positive z is negative
	float_t y3 = o.y() - _radius * sinf(m_fTotalRotation + ((5.0f * M3D_PI) / 4.0f));

	float_t x4 = o.z() - _radius * cosf(m_fTotalRotation + ((7.0f * M3D_PI) / 4.0f)); // subtraction here because positive z is negative
	float_t y4 = o.y() - _radius * sinf(m_fTotalRotation + ((7.0f * M3D_PI) / 4.0f));


	glLoadIdentity();

	glColor3f(1.0f,0.0f,0.0f);  

	glPolygonMode( GL_FRONT_AND_BACK, GL_LINE );

	glBegin(GL_QUADS);

	
	glVertex3f(-x4, -y4, 0);
	glVertex3f(-x3, -y3, 0);
	glVertex3f(-x2, -y2, 0);
	glVertex3f(-x1, -y1, 0);

	glEnd();

	// Render solid

	_halfWidths = ((btBoxShape*)m_physicsObject->body->getCollisionShape())->getHalfExtentsWithoutMargin();

	_halfWidths.setX(_halfWidths.x());
	_halfWidths.setY(_halfWidths.y());
	_halfWidths.setZ(_halfWidths.z());

	// Find box "radius"
	_radius = sqrtf(powf(_halfWidths.z(), 2) + powf(_halfWidths.y(), 2));

	// Calculate rotated points
	x1 = o.z() - _radius * cosf(m_fTotalRotation + (M3D_PI / 4.0f)); // subtraction here because positive z is negative
	y1 = o.y() - _radius * sinf(m_fTotalRotation + (M3D_PI / 4.0f));

	x2 = o.z() - _radius * cosf(m_fTotalRotation + ((3.0f * M3D_PI) / 4.0f)); // subtraction here because positive z is negative
	y2 = o.y() - _radius * sinf(m_fTotalRotation + ((3.0f * M3D_PI) / 4.0f));

	x3 = o.z() - _radius * cosf(m_fTotalRotation + ((5.0f * M3D_PI) / 4.0f)); // subtraction here because positive z is negative
	y3 = o.y() - _radius * sinf(m_fTotalRotation + ((5.0f * M3D_PI) / 4.0f));

	x4 = o.z() - _radius * cosf(m_fTotalRotation + ((7.0f * M3D_PI) / 4.0f)); // subtraction here because positive z is negative
	y4 = o.y() - _radius * sinf(m_fTotalRotation + ((7.0f * M3D_PI) / 4.0f));

	glPolygonMode( GL_FRONT_AND_BACK, GL_FILL );

	glBegin(GL_QUADS);

	glVertex3f(-x4, -y4, 0);
	glVertex3f(-x3, -y3, 0);
	glVertex3f(-x2, -y2, 0);
	glVertex3f(-x1, -y1, 0);


	glEnd();

}

